#include "Drv_UP_flow.h"
#include "Drv_Usart.h"
#include "Drv_time.h"



#define delay_ms Delay_ms
#define Drv_Uart4SendBuf Uart4_Send

const static uint8_t Sensor_cfg[]={
0x12, 0x80, 
0x11, 0x30, 
0x1b, 0x06, 
0x6b, 0x43, 
0x12, 0x20, 
0x3a, 0x00, 
0x15, 0x02, 
0x62, 0x81, 
0x08, 0xa0, 
0x06, 0x68, 
0x2b, 0x20, 
0x92, 0x25, 
0x27, 0x97, 
0x17, 0x01, 
0x18, 0x79, 
0x19, 0x00, 
0x1a, 0xa0, 
0x03, 0x00, 
0x13, 0x00, 
0x01, 0x13, 
0x02, 0x20, 
0x87, 0x16, 
0x8c, 0x01, 
0x8d, 0xcc, 
0x13, 0x07, 
0x33, 0x10, 
0x34, 0x1d, 
0x35, 0x46, 
0x36, 0x40, 
0x37, 0xa4, 
0x38, 0x7c, 
0x65, 0x46, 
0x66, 0x46, 
0x6e, 0x20, 
0x9b, 0xa4, 
0x9c, 0x7c, 
0xbc, 0x0c, 
0xbd, 0xa4, 
0xbe, 0x7c, 
0x20, 0x09, 
0x09, 0x03, 
0x72, 0x2f, 
0x73, 0x2f, 
0x74, 0xa7, 
0x75, 0x12, 
0x79, 0x8d, 
0x7a, 0x00, 
0x7e, 0xfa, 
0x70, 0x0f, 
0x7c, 0x84, 
0x7d, 0xba, 
0x5b, 0xc2, 
0x76, 0x90, 
0x7b, 0x55, 
0x71, 0x46, 
0x77, 0xdd, 
0x13, 0x0f, 
0x8a, 0x10, 
0x8b, 0x20, 
0x8e, 0x21, 
0x8f, 0x40, 
0x94, 0x41, 
0x95, 0x7e, 
0x96, 0x7f, 
0x97, 0xf3, 
0x13, 0x07, 
0x24, 0x58, 
0x97, 0x48, 
0x25, 0x08, 
0x94, 0xb5, 
0x95, 0xc0, 
0x80, 0xf4, 
0x81, 0xe0, 
0x82, 0x1b, 
0x83, 0x37, 
0x84, 0x39, 
0x85, 0x58, 
0x86, 0xff, 
0x89, 0x15, 
0x8a, 0xb8, 
0x8b, 0x99, 
0x39, 0x98, 
0x3f, 0x98, 
0x90, 0xa0, 
0x91, 0xe0, 
0x40, 0x20, 
0x41, 0x28, 
0x42, 0x26, 
0x43, 0x25, 
0x44, 0x1f, 
0x45, 0x1a, 
0x46, 0x16, 
0x47, 0x12, 
0x48, 0x0f, 
0x49, 0x0d, 
0x4b, 0x0b, 
0x4c, 0x0a, 
0x4e, 0x08, 
0x4f, 0x06, 
0x50, 0x06, 
0x5a, 0x56, 
0x51, 0x1b, 
0x52, 0x04, 
0x53, 0x4a, 
0x54, 0x26, 
0x57, 0x75, 
0x58, 0x2b, 
0x5a, 0xd6, 
0x51, 0x28, 
0x52, 0x1e, 
0x53, 0x9e, 
0x54, 0x70, 
0x57, 0x50, 
0x58, 0x07, 
0x5c, 0x28, 
0xb0, 0xe0, 
0xb1, 0xc0, 
0xb2, 0xb0, 
0xb3, 0x4f, 
0xb4, 0x63, 
0xb4, 0xe3, 
0xb1, 0xf0, 
0xb2, 0xa0, 
0x55, 0x00, 
0x56, 0x40, 
0x96, 0x50, 
0x9a, 0x30, 
0x6a, 0x81, 
0x23, 0x33, 
0xa0, 0xd0, 
0xa1, 0x31, 
0xa6, 0x04, 
0xa2, 0x0f, 
0xa3, 0x2b, 
0xa4, 0x0f, 
0xa5, 0x2b, 
0xa7, 0x9a, 
0xa8, 0x1c, 
0xa9, 0x11, 
0xaa, 0x16, 
0xab, 0x16, 
0xac, 0x3c, 
0xad, 0xf0, 
0xae, 0x57, 
0xc6, 0xaa, 
0xd2, 0x78, 
0xd0, 0xb4, 
0xd1, 0x00, 
0xc8, 0x10, 
0xc9, 0x12, 
0xd3, 0x09, 
0xd4, 0x2a, 
0xee, 0x4c, 
0x7e, 0xfa, 
0x74, 0xa7, 
0x78, 0x4e, 
0x60, 0xe7, 
0x61, 0xc8, 
0x6d, 0x70, 
0x1e, 0x39, 
0x98, 0x1a
};

#define CIRCLE_P(n,a,b) ((a) + ((n)-(a))%((b)-(a)))
#define OF_BUFFER_NUM 14

uint8_t of_init_cnt;
uint8_t of_buf_update_cnt;
uint8_t OF_DATA[OF_BUFFER_NUM];


static uint8_t com_getdata[OF_BUFFER_NUM]= {0};//buffer

u8 Drv_OFInit(void)
{
	uint8_t send_buffer[10];
	uint8_t i;
	//rst
	of_init_cnt = 0;
	
	//
	send_buffer[0] = 0xAA;
	Drv_Uart4SendBuf(send_buffer, 1);

	send_buffer[0] = 0xAB;
	send_buffer[1] = 0x96;
	send_buffer[2] = 0x26;
	send_buffer[3] = 0xbc;
	send_buffer[4] = 0x50;
	send_buffer[5] = 0x5c;
	Drv_Uart4SendBuf(send_buffer, 6);

	//check_ack
	delay_ms(10);
	//
	if (of_init_cnt >= 3)
	{
		if (com_getdata[0] == 0xab && com_getdata[1] == 0x00 && com_getdata[2] == 0xab)
		{
			of_init_cnt = 0;
		}
		else
		{
			of_init_cnt = 10;
		}
	}
	else
	{
		of_init_cnt = 10;
	}
		
	//
	uint8_t len = 161;
	for(i=0; i<len;i+=2 )
	{
		send_buffer[0] = 0xBB;	 
		send_buffer[1] = 0xdc;	
		send_buffer[2] = Sensor_cfg[i];
		send_buffer[3] = Sensor_cfg[i+1];
		send_buffer[4] = (0xdc^send_buffer[2]^send_buffer[3]);
		Drv_Uart4SendBuf(send_buffer, 5);

		//check_ack
		delay_ms(10);
		//
		if(of_init_cnt >= 3)
		{
			if (com_getdata[0] == 0xbb && com_getdata[1] == 0x00 && com_getdata[2] == 0xbb)
			{
				of_init_cnt = 0;

			}
			else
			{
				of_init_cnt = 10;

			}
		}
		else
		{
			of_init_cnt = 10;
		}
				
	}
	
	send_buffer[0] = 0xDD;
	//
	Drv_Uart4SendBuf(send_buffer, 1);

	
	
	if(of_init_cnt == 10)
	{
		//光流初始化检测
//		while(1)
//		{
//				for(u8 i = 0;i<2;i++)
//				{
//					led_on(LED_ALL);
//					//
//					delay_ms(200);
//					//
//					led_off(LED_ALL);
//					//
//					delay_ms(200);
//				}
//				//
//				delay_ms(1500);
//				//			
//		}
		return (0);
	}
	else
	{
		//
		of_init_cnt = 255;
		//
		//of_init_type = 2;
		return (1);
	}
	
	
}


void OFGetByte(uint8_t data)
{
	static uint8_t in_p=0,head_p = 0;
	
	//
	if(of_init_cnt!=255 )
	{
		if(of_init_cnt<OF_BUFFER_NUM)
		{
			com_getdata[of_init_cnt++] = data;
		}
	}
	else	
	{
		//		
		com_getdata[in_p++] = data;
		in_p %= OF_BUFFER_NUM;

		if(in_p == head_p)
		{
			if(
						 (com_getdata[CIRCLE_P(head_p   ,0,OF_BUFFER_NUM)] == 0xFE)
					&& (com_getdata[CIRCLE_P(head_p+1 ,0,OF_BUFFER_NUM)] == 0x0A)
					&& (com_getdata[CIRCLE_P(head_p+13,0,OF_BUFFER_NUM)] == 0x55)
				)
			{
				for(uint8_t i=0; i<OF_BUFFER_NUM; i++)
				{
					OF_DATA[i] = com_getdata[CIRCLE_P(head_p+i,0,OF_BUFFER_NUM)];
				}
				of_buf_update_cnt++;

			}
			else
			{		
				head_p ++;
				head_p %= OF_BUFFER_NUM;
			}
		}
	}
}

